package il.ac.tau.cvws.scrabblesidekick.server.engine.recoginition.common.rectifier;

import il.ac.tau.cvws.scrabblesidekick.server.app.manager.GameManager;
import il.ac.tau.cvws.scrabblesidekick.server.constant.FileConstants;
import il.ac.tau.cvws.scrabblesidekick.server.engine.recoginition.calibration.common.CalibrationConstants;
import il.ac.tau.cvws.scrabblesidekick.server.util.cv.CvUtil;
import il.ac.tau.cvws.scrabblesidekick.server.util.debug.DebugUtil;
import il.ac.tau.cvws.scrabblesidekick.server.util.debug.DebugUtil.DebugTypes;
import il.ac.tau.cvws.scrabblesidekick.shared.constant.BoardConstants;
import il.ac.tau.cvws.scrabblesidekick.shared.datatype.game.Board;
import il.ac.tau.cvws.scrabblesidekick.shared.util.math.Vector;

import java.util.Arrays;
import java.util.Collection;
import java.util.SortedMap;
import java.util.TreeMap;

import com.googlecode.javacv.JavaCV;
import com.googlecode.javacv.cpp.opencv_core;
import com.googlecode.javacv.cpp.opencv_core.CvMat;
import com.googlecode.javacv.cpp.opencv_core.CvPoint;
import com.googlecode.javacv.cpp.opencv_core.CvScalar;
import com.googlecode.javacv.cpp.opencv_core.IplImage;
import com.googlecode.javacv.cpp.opencv_imgproc;

public class BoardRectifier
{	
	private final int state;
	
	public BoardRectifier()
	{
		this(BoardRectifierConstants.TILT_STATE);
	}
	
	public BoardRectifier(int state)
	{
		this.state = state;
	}
	
	public IplImage rectify(IplImage sceneImage, int screenWidth, int screenHeight)
	{
		switch (state)
		{
			case BoardRectifierConstants.RECT_STATE:
			{
				return cropRectifiedBoard(sceneImage);
			}
			case BoardRectifierConstants.TILT_STATE:
			{
				return cropTiltedBoard(sceneImage, screenWidth, screenHeight);
			}
			case BoardRectifierConstants.FREE_STATE:
			{
				return rectifyBoardImage(sceneImage);
			}
			
			default: return null;
		}
	}
	
	protected IplImage cropTiltedBoard(IplImage sceneImage, int screenWidth, int screenHeight)
	{
		double nHeight = sceneImage.width() * ((double)screenHeight / screenWidth);
		double heightBevel = ((sceneImage.height() - nHeight) / 2.0);
		
		IplImage croppedImage = CvUtil.cropImage(sceneImage, 0, (int)heightBevel, sceneImage.width(), (int)nHeight);
		DebugUtil.saveImage(croppedImage, DebugTypes.RECTIFIER, "cropedImage", true);
		
		double imageScale = ((double)screenHeight / croppedImage.height());
		
		int smallWidth = (int)(imageScale * croppedImage.width());
		int smallHeight = (int)(imageScale * croppedImage.height());
		
		IplImage smallImage = CvUtil.resizeImage(croppedImage, smallWidth, smallHeight);
		DebugUtil.saveImage(smallImage, DebugTypes.RECTIFIER, "small", true);
		
		CvPoint[] corners = findTiltedCorners(smallImage);
		
		if (corners == null)
		{
			return null;
		}
		
		if (GameManager.instance().debugMode())
		{
			CvUtil.drawCircle(smallImage, corners[0].x(), corners[0].y(), 10);
			CvUtil.drawCircle(smallImage, corners[1].x(), corners[1].y(), 10);
			CvUtil.drawCircle(smallImage, corners[2].x(), corners[2].y(), 10);
			CvUtil.drawCircle(smallImage, corners[3].x(), corners[3].y(), 10);
			
			CvUtil.drawCircle(smallImage, corners[4].x(), corners[4].y(), 20);
			CvUtil.drawCircle(smallImage, corners[5].x(), corners[5].y(), 20);
			CvUtil.drawCircle(smallImage, corners[6].x(), corners[6].y(), 20);
			CvUtil.drawCircle(smallImage, corners[7].x(), corners[7].y(), 20);
			
			DebugUtil.saveImage(smallImage, DebugTypes.RECTIFIER, "new_corners", true);	
		}
		
		for (int i = 0; i < corners.length; i++)
		{
			int sceneX = (int)(corners[i].x() / imageScale);
			int sceneY = (int)(corners[i].y() / imageScale);
			
			corners[i] = opencv_core.cvPoint(sceneX, sceneY);
		}
		
		IplImage rectifiedBoard = rectifyRectifiedBoard(croppedImage, corners);
		
		return rectifiedBoard;
	}
	
	private CvPoint[] findTiltedCorners(IplImage sceneImage)
	{
		IplImage thresholdImage = CvUtil.createThresholdImage(sceneImage, 0.5);
		DebugUtil.saveImage(thresholdImage, DebugTypes.RECTIFIER, "threshold", true);
		
		int midPointX = (int)(sceneImage.width() / 2.0);
 		
 		int y0 		  = (int)(sceneImage.height() / 12.0);
 		int rectWidth = (int)(sceneImage.height() - 2.0 * y0);
 		int x0 		  = (int)(midPointX - (rectWidth / 2.0));
		
 		int x1 = x0 + rectWidth;
 		int y1 = y0;
 		
 		int x2 = x0 + rectWidth;
 		int y2 = y0 + rectWidth;
 		
 		int x3 = x0;
 		int y3 = y0 + rectWidth;
		
 		int innerRadius = sceneImage.height() / 20;
 		int outerRadius = innerRadius;
 		
 		int ux = 0;
 		int vx = 0;
 		
 		boolean found = false;
 		
 		double maxRatio = Double.MIN_VALUE;
 		double ratio = 0;
 		
 		for (ux = x0 + innerRadius; (ux > x0 - outerRadius) && !found; ux--)
 		{
 			for (vx = x3 + innerRadius; (vx > x3 - outerRadius) && !found; vx--)
 			{
 				ratio = blackLineRatio(thresholdImage, ux, y0, vx, y3);
 				
 				maxRatio = Math.max(ratio, maxRatio);
 				
 				if (ratio > 0.98)
 				{
 					found = true;
 				}
 			}
 		}
 		
 		if (!found)
 		{
 			return null;
 		}
 		
 		int sx = 0;
 		int tx = 0;
 		
 		found = false;
 		
 		for (sx = x1 - innerRadius; (sx < x1 + outerRadius) && !found; sx++)
 		{
 			for (tx = x2 - innerRadius; (tx < x2 + outerRadius) && !found; tx++)
 			{
 				if (blackLineRatio(thresholdImage, sx, y1, tx, y2) > 0.98)
 				{
 					found = true;
 				}
 			}
 		}
 		
 		if (!found)
 		{
 			return null;
 		}
 		
 		int uy = 0;
 		int sy = 0;
 		
 		found = false;
 		
 		for (uy = y0 + innerRadius; (uy > y0 - outerRadius) && !found; uy--)
 		{
 			for (sy = y1 + innerRadius; (sy > y1 - outerRadius) && !found; sy--)
 			{
 				if (blackLineRatio(thresholdImage, ux, uy, sx, sy) > 0.98)
 				{
 					found = true;
 				}
 			}
 		}
 		
 		if (!found)
 		{
 			return null;
 		}
 		
 		int vy = 0;
 		int ty = 0;
 		
 		found = false;
 		
 		for (vy = y3 - innerRadius; (vy < y3 + outerRadius) && !found; vy++)
 		{
 			for (ty = y2 - innerRadius; (ty < y2 + outerRadius) && !found; ty++)
 			{
 				if (blackLineRatio(thresholdImage, vx, vy, tx, ty) > 0.98)
 				{
 					found = true;
 				}
 			}
 		}
 		
 		if (!found)
 		{
 			return null;
 		}
 		
 		CvPoint[] result = new CvPoint[8];
 		
 		result[0] = opencv_core.cvPoint(ux, uy);
 		result[1] = opencv_core.cvPoint(sx, sy);
 		result[2] = opencv_core.cvPoint(tx, ty);
 		result[3] = opencv_core.cvPoint(vx, vy);
 		
 		result[4] = opencv_core.cvPoint(x0, y0);
 		result[5] = opencv_core.cvPoint(x1, y1);
 		result[6] = opencv_core.cvPoint(x2, y2);
 		result[7] = opencv_core.cvPoint(x3, y3);
 		
 		return result;
	}
	
	private double blackLineRatio(IplImage image, int x1, int y1, int x2, int y2)
	{
		int dx, sx;
		int dy, sy;
		
		if (x1 < x2)
		{
			dx = x2 - x1;
			sx = 1;
		}
		else
		{
			dx = x1 - x2;
			sx = -1;
		}
		
		if (y1 < y2)
		{
			dy = y2 - y1;
			sy = 1;
		}
		else
		{
			dy = y1 - y2;
			sy = -1;
		}
		
		int err = dx - dy;
		
		int e2;
		
		int totalCount = 0;
		int blackCount = 0;
		
		while (true)
		{
			CvScalar p = opencv_core.cvGet2D(image, y1, x1);
			
			if (p.magnitude() < 100.0)
			{
				blackCount++;
			}
			
			totalCount++;
			
			if ((x1 == x2) && (y1 == y2))
			{
				break;
			}
			
			e2 = 2 * err;
			
			if (e2 > -dy)
			{
				err -= dy;
				x1 += sx;
			}
			
			if (e2 < dx)
			{
				err += dx;
				y1 += sy;
			}
		}
		
		return ((double)blackCount / totalCount);
	}
	
	protected IplImage cropRectifiedBoard(IplImage sceneImage)
	{
		CvPoint p0 = new CvPoint(BoardConstants.TOP_CORNER_X, BoardConstants.TOP_CORNER_Y);
		CvPoint p1 = new CvPoint(BoardConstants.TOP_CORNER_X + BoardConstants.WIDTH, BoardConstants.TOP_CORNER_Y);
		CvPoint p2 = new CvPoint(BoardConstants.TOP_CORNER_X, BoardConstants.TOP_CORNER_Y + BoardConstants.HEIGHT);
		CvPoint p3 = new CvPoint(BoardConstants.TOP_CORNER_X + BoardConstants.WIDTH, BoardConstants.TOP_CORNER_Y + BoardConstants.HEIGHT);
		
		return rectifyRectifiedBoard(sceneImage, new CvPoint[]{p0, p1, p3, p2});
	}
	
	protected IplImage rectifyRectifiedBoard(IplImage sceneImage, CvPoint[] points)
	{
		int bevelX = -(int)(CalibrationConstants.BOARD_IMAGE_WIDTH * BoardConstants.INNER_BOARD_HORIZONTAL_BEVEL_RATIO);
		int bevelY = -(int)(CalibrationConstants.BOARD_IMAGE_HEIGHT * BoardConstants.INNER_BOARD_VERTICAL_BEVEL_RATIO);
		
		bevelX = bevelX + (int)((double)CalibrationConstants.BOARD_IMAGE_WIDTH / BoardConstants.BOARD_TILES_NUM);
		bevelY = bevelY + (int)((double)CalibrationConstants.BOARD_IMAGE_HEIGHT / BoardConstants.BOARD_TILES_NUM);
		
		IplImage rectifiedImage = createRectifiedImage(sceneImage, points, bevelX, bevelY);
		DebugUtil.saveImage(rectifiedImage, DebugTypes.RECTIFIER, "rectified", true);
		
		return rectifiedImage;
	}
	
	protected IplImage rectifyBoardImage(IplImage sceneImage)
	{
		IplImage dotImage = CvUtil.loadImage(FileConstants.SYNC_DIR_NAME + FileConstants.SYNC_DOT_NAME);
		
		int dotOffsetX = dotImage.width() / 2;
		int dotOffsetY = dotImage.height() / 2;
		
		IplImage bwSceneImage = CvUtil.erodeImage(sceneImage, 2);

		DebugUtil.saveImage(bwSceneImage, DebugTypes.RECTIFIER, "erode_image", true);	
		
		bwSceneImage = CvUtil.createThresholdImage(bwSceneImage, 0.5, true, true);
		
		DebugUtil.saveImage(bwSceneImage, DebugTypes.RECTIFIER, "threshold_image", true);	
		
		CvMat matchResult = CvMat.create(
				bwSceneImage.height() - dotImage.height() + 1,
				bwSceneImage.width() - dotImage.width() + 1,
				opencv_core.CV_32F);
		
		opencv_imgproc.cvMatchTemplate(bwSceneImage, dotImage, matchResult, opencv_imgproc.CV_TM_SQDIFF);
		opencv_core.cvNormalize(matchResult, matchResult, 1.0, 0.0, opencv_core.CV_MINMAX, null);
		
		SortedMap<Double, CvPoint> goodPoints = new TreeMap<Double, CvPoint>();
		
		for (int row = 0; row < matchResult.rows(); row++)
		{
			for (int col = 0; col < matchResult.cols(); col++)
			{
				goodPoints.put(matchResult.get(row, col),
						opencv_core.cvPoint(col + dotOffsetX, row + dotOffsetY));
				
				if (goodPoints.size() > BoardRectifierConstants.MAX_SYNC_DOT_COUNT)
				{
					goodPoints.remove(goodPoints.lastKey());
				}
			}
		}
		
		IplImage sceneDebugImage = bwSceneImage.clone();
		
		for (CvPoint point : goodPoints.values())
		{
			CvUtil.drawRect(sceneDebugImage,
					point.x() - dotOffsetX,
					point.y() - dotOffsetY, dotImage.width(), dotImage.height());
		}
		
		CvPoint[] syncLines = findSyncLines(sceneDebugImage, goodPoints.values(), bwSceneImage.height());
		
		DebugUtil.saveImage(sceneDebugImage, DebugTypes.RECTIFIER, "rectifier_debug", true);
		
		if (syncLines == null)
		{
			return null;
		}
		
		CvPoint[] cornerPoints = sortCorners(syncLines, bwSceneImage.width(), bwSceneImage.height());
		
		int bevelX = (int)(CalibrationConstants.BOARD_IMAGE_WIDTH * (0.25 / (Board.SQUARES_NUM - 1)));
		int bevelY = (int)(CalibrationConstants.BOARD_IMAGE_HEIGHT * (2.125 / (Board.SQUARES_NUM - 1)));
		
		IplImage rectifiedImage = createRectifiedImage(sceneImage, cornerPoints, bevelX, bevelY);
		
		return rectifiedImage;
	}
	
	private CvPoint[] findSyncLines(IplImage debugImage, Collection<CvPoint> points, int referenceSize)
	{
		CvPoint[] midPoints = new CvPoint[BoardRectifierConstants.COLLINEAR_SYNC_DOT_COUNT];
		CvPoint[] syncLines = new CvPoint[4];
		
		double maxDistRatio = BoardRectifierConstants.MAX_DISTANCE_RATIO;
		
		for (CvPoint p1 : points)
		{
			double x1 = p1.x();
			double y1 = p1.y();
			
			for (CvPoint p2 : points)
			{
				double x2 = p2.x();
				double y2 = p2.y();
				
				double d12 = Vector.distance(x1, y1, x2, y2);
				
				if (d12 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON)
				{
					continue;
				}
				
				if (syncLines[0] != null)
				{
					double d1 = Vector.distance(x1, y1, syncLines[0].x(), syncLines[0].y());
					double d2 = Vector.distance(x1, y1, syncLines[1].x(), syncLines[1].y());
					double d3 = Vector.distance(x2, y2, syncLines[0].x(), syncLines[0].y());
					double d4 = Vector.distance(x2, y2, syncLines[1].x(), syncLines[1].y());
					
					if ((d1 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) ||
						(d2 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) ||
						(d3 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) ||
						(d4 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON))
					{
						continue;
					}
				}
				
				Arrays.fill(midPoints, null);
				
				midPoints[0] = p1;
				midPoints[1] = p2;
				
				for (CvPoint p3 : points)
				{
					double x3 = p3.x();
					double y3 = p3.y();
					
					double d13 = Vector.distance(x1, y1, x3, y3);
					double d23 = Vector.distance(x2, y2, x3, y3);
					
					if ((d13 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) ||
						(d23 < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) ||
						(d13 > d12) || ((d12 / d13) > maxDistRatio) ||
						(d23 > d12) || ((d12 / d23) > maxDistRatio))
					{
						continue;
					}
					
					double dot1 = Vector.dot(x2 - x1, y2 - y1, x3 - x1, y3 - y1);
					double dot2 = Vector.dot(x1 - x2, y1 - y2, x3 - x2, y3 - y2);
					
					double cangle1 = Math.abs(1 - (dot1 / (d12 * d13)));
					double cangle2 = Math.abs(1 - (dot2 / (d12 * d23)));
					
					if ((cangle1 < BoardRectifierConstants.COLLINEARITY_COSANGLE_EPSILON) &&
						(cangle2 < BoardRectifierConstants.COLLINEARITY_COSANGLE_EPSILON))
					{
						for (int i = 0; i < midPoints.length; i++)
						{
							if (midPoints[i] == null)
							{
								midPoints[i] = p3;
								
								if (i == (midPoints.length - 1))
								{
									if (syncLines[0] == null)
									{
										syncLines[0] = midPoints[0];
										syncLines[1] = midPoints[1];
										
										drawSyncLine(debugImage, midPoints);
									}
									else
									{
										syncLines[2] = midPoints[0];
										syncLines[3] = midPoints[1];
										
										drawSyncLine(debugImage, midPoints);
										
										return syncLines;
									}
								}
								
								break;
							}
							
							double mx = midPoints[i].x();
							double my = midPoints[i].y();
							
							double md = Vector.distance(x3, y3, mx, my);
							
							if (md < BoardRectifierConstants.COLLINEARITY_DISTANCE_EPSILON) //TODO: make this relative to the provided referenceSize
							{
								break;
							}
						}
					}
				}
			}
		}
		
		return null;
	}
	
	private CvPoint[] sortCorners(CvPoint[] syncLines, double imageWidth, double imageHeight)
	{
		CvPoint[] corners = new CvPoint[4];
		
		double lmx = 0.0;
		double lmy = imageHeight / 2.0;
		double rmx = imageWidth - 1.0;
		double rmy = imageHeight / 2.0;
		
		double mx1 = (syncLines[0].x() + syncLines[1].x()) / 2.0;
		double my1 = (syncLines[0].y() + syncLines[1].y()) / 2.0;
		double mx2 = (syncLines[2].x() + syncLines[3].x()) / 2.0;
		double my2 = (syncLines[2].y() + syncLines[3].y()) / 2.0;
		
		double dl1 = Vector.distance(mx1, my1, lmx, lmy);
		double dl2 = Vector.distance(mx2, my2, lmx, lmy);
		
		if (dl1 > dl2)
		{
			CvPoint t1 = syncLines[0];
			CvPoint t2 = syncLines[1];
			
			syncLines[0] = syncLines[2];
			syncLines[1] = syncLines[3];
			
			syncLines[2] = t1;
			syncLines[3] = t2;
		}
		
		double zl = Vector.cross(
				syncLines[0].x() - syncLines[1].x(),
				syncLines[0].y() - syncLines[1].y(),
				lmx - syncLines[1].x(),
				lmy - syncLines[1].y());
		
		if (zl < 0.0)
		{
			corners[0] = syncLines[0];
			corners[3] = syncLines[1];
		}
		else
		{
			corners[0] = syncLines[1];
			corners[3] = syncLines[0];
		}
		
		double zr = Vector.cross(
				syncLines[3].x() - syncLines[2].x(),
				syncLines[3].y() - syncLines[2].y(),
				rmx - syncLines[2].x(),
				rmy - syncLines[2].y());
		
		if (zr < 0.0)
		{
			corners[1] = syncLines[2];
			corners[2] = syncLines[3];
		}
		else
		{
			corners[1] = syncLines[3];
			corners[2] = syncLines[2];
		}
		
		return corners;
	}
	
	private IplImage createRectifiedImage(IplImage image, CvPoint[] corners, int bevelX, int bevelY)
	{
		int boardWidth = CalibrationConstants.BOARD_IMAGE_WIDTH;
		int boardHeight = CalibrationConstants.BOARD_IMAGE_HEIGHT;
		
		double[] sourcePoints = {
				corners[0].x(), corners[0].y(),
				corners[1].x(), corners[1].y(),
				corners[2].x(), corners[2].y(),
				corners[3].x(), corners[3].y() };
		
		double[] targetPoints = {
				bevelX, bevelY,
				boardWidth - bevelX, bevelY,
				boardWidth - bevelX, boardHeight - bevelY,
				bevelX, boardHeight - bevelY };
		
		CvMat perspectiveMat = CvMat.create(3, 3, opencv_core.CV_32F);
		JavaCV.getPerspectiveTransform(sourcePoints, targetPoints, perspectiveMat);
		
		IplImage boardImage = CvUtil.warpImagePerspective(image, boardWidth, boardHeight, perspectiveMat);
		
		return boardImage;
	}
	
	private void drawSyncLine(IplImage image, CvPoint[] line)
	{
		CvUtil.drawLine(image,
				line[0].x(), line[0].y(),
				line[1].x(), line[1].y());
		
		for (int i = 0; i < line.length; i++)
		{
			CvUtil.drawCircle(image, line[i].x(), line[i].y(), 15, 127.0, 0.0, 127.0);
		}
	}
}
